#!/bin/bash

echo "启动ROS导航系统..."

# 设置工作空间环境
source /opt/ros/noetic/setup.bash
source /home/af/RoboMaster/mid360/catkin_ws/devel/setup.bash

# 启动ROS核心
echo "启动ROS核心..."
roscore &
ROS_CORE_PID=$!
sleep 3

# 启动地图服务器
echo "启动地图服务器..."
rosrun map_server map_server /home/af/RoboMaster/mid360/catkin_ws/src/NEXTE_Sentry_Nav/src/sentry_slam/FAST_LIO/PCD/scans.yaml &
MAP_SERVER_PID=$!
sleep 2

# 启动静态TF转换
echo "启动TF转换..."
rosrun tf static_transform_publisher 0 0 0 0 0 0 map odom 100 &
TF_MAP_ODOM_PID=$!
rosrun tf static_transform_publisher 0 0 0 0 0 0 base_link laser 100 &
TF_BASE_LASER_PID=$!
sleep 1

# 启动move_base路径规划
echo "启动路径规划..."
roslaunch sentry_nav sentry_navigation.launch &
MOVE_BASE_PID=$!

# 等待用户输入
echo ""
echo "导航系统已启动完成！"
echo "现在可以在RViz中设置目标点进行路径规划"
echo ""
echo "按Ctrl+C停止所有服务"

# 等待用户中断
wait $MOVE_BASE_PID

# 清理进程
echo "停止所有服务..."
kill $MOVE_BASE_PID 2>/dev/null
kill $TF_BASE_LASER_PID 2>/dev/null
kill $TF_MAP_ODOM_PID 2>/dev/null
kill $MAP_SERVER_PID 2>/dev/null
kill $ROS_CORE_PID 2>/dev/null

echo "导航系统已停止"